#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include "mytest.h"


void my_SIFTTest(cv::Mat &image, cv::Mat &temp, int method, bool bResize) 
{ 
    cv::Mat result;
    
    std::string windowName = "SIFT Test";
    cv::namedWindow(windowName, CV_WINDOW_NORMAL);
    int recommended_width = 800, recommended_height=600;
//     int thresh = 127, kernelSize=3;
//     double requiredConfidence=0.93;
    cv::Scalar color(255, 0, 0);
    double minVal, maxVal; 
    cv::Point minLoc, maxLoc, matchPoint;
//     my_convertToBinary(temp, temp);
//     my_openImage(temp, kernelSize);
//     cv::Canny(temp, temp, 200, 20);
    //my_convertToGray8Bit(temp);
   // cv::equalizeHist(temp, temp);
    //THIS IS VERY GOOD DON'T TOUCH IT!!!
    cv::FastFeatureDetector SIFTDetector(10);
   // cv::SurfFeatureDetector SIFTDetector(400);
    bool SURF=false; //SET THIS IF SURF
    std::vector<cv::KeyPoint>  keypoints_temp, keypoints_image;
    SIFTDetector.detect(temp, keypoints_temp);
    cv::SiftDescriptorExtractor SIFTExtractor;
    cv::Mat descriptors_temp, descriptors_image;
    SIFTExtractor.compute(temp, keypoints_temp, descriptors_temp);
//     cv::BFMatcher matcher;
    cv::FlannBasedMatcher matcher;
    std::vector<cv::DMatch> matches;
  std::vector<cv::Point2f> scene_corners(4);
  bool bFound;
  
      if(bResize) cv::resize(image, image, cv::Size(recommended_width, recommended_height));
	bFound = false;
        cv::Mat image_source  = image;
	//my_convertToGray8Bit(image); //convert to gray
	cv::equalizeHist(image, image); //equalize hist
	
	//if SURF 
// 	if(SURF)
// 	{
// 	  cv::SurfFeatureDetector feature(300);
// 	  feature.detect(image, keypoints_image);
// 	}else
	  SIFTDetector.detect(image, keypoints_image);
	SIFTExtractor.compute(image, keypoints_image, descriptors_image);
	matcher.match(descriptors_temp, descriptors_image, matches);
	  //-- Quick calculation of max and min distances between keypoints
	 double max_dist = 0; double min_dist = 100;
      for( int i = 0; i < descriptors_temp.rows; i++ )
      { double dist = matches[i].distance;
	if( dist < min_dist ) min_dist = dist;
	if( dist > max_dist ) max_dist = dist;
      }

  //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
  std::vector< cv::DMatch > good_matches;

  for( int i = 0; i < descriptors_temp.rows; i++ )
  { 
    if( matches[i].distance < 5*min_dist )
     { good_matches.push_back( matches[i]); }
  }
  //good_matches = matches;
	if(good_matches.size() >= 5)
	{ 
	  bFound = true;
	
	std::vector<cv::Point2f> obj;
	std::vector<cv::Point2f> scene;
	for( int i = 0; i < good_matches.size(); i++ )
	{
	  //-- Get the keypoints from the good matches
	  obj.push_back( keypoints_temp[ good_matches[i].queryIdx ].pt );
	  scene.push_back( keypoints_image[ good_matches[i].trainIdx ].pt );
	}
	//cv::Mat H = cv::findHomography(obj, scene, CV_RANSAC);
	cv::Mat H = cv::findHomography(obj, scene, CV_RANSAC, 10); //7-20 seems good
	//20 for im5 only, perfect
	//7 for im3
	//10 is perfect for FAST
	
	
	//-- Get the corners from the image_1 ( the object to be "detected" )
	std::vector<cv::Point2f> obj_corners(4);
	obj_corners[0] = cv::Point2f(0,0); obj_corners[1] = cv::Point2f( temp.cols, 0 );
	obj_corners[2] = cv::Point2f( temp.cols, temp.rows ); obj_corners[3] = cv::Point2f( 0, temp.rows );

	perspectiveTransform( obj_corners, scene_corners, H);
      if(bFound) {
  cv::line( image, scene_corners[0] , scene_corners[1]  , cv::Scalar(255, 0, 0), 2 );
  cv::line( image, scene_corners[1] , scene_corners[2]  , cv::Scalar( 255, 0, 0), 2 );
  cv::line( image, scene_corners[2] , scene_corners[3]  , cv::Scalar( 255, 0, 0), 2 );
  cv::line( image, scene_corners[3] , scene_corners[0]  , cv::Scalar( 255, 0, 0), 2 );
      }
	}	
      
	//my_convertToBinary(frame, frame, thresh);
//       if(maxVal > requiredConfidence)
//  	cv::rectangle(frame_bin, matchPoint, cv::Point(matchPoint.x+temp.cols, matchPoint.y+temp.rows), color, 2, 8, 0);
      cv::drawMatches(temp, keypoints_temp, image, keypoints_image, good_matches, image_source);
      
      //cv::drawMatches(image, keypoints_image, temp, keypoints_temp, matches, image_source);
      cv::imshow(windowName, image_source);
    // cv::imshow(windowName, descriptors_image);
      cv::waitKey();
    
}






#include <stdio.h>
#include <iostream>
#include "opencv2/calib3d/calib3d.hpp"

using namespace cv;

void readme();

int my_SURFTest( cv::Mat &img_scene, cv::Mat &img_object)
{

  if( !img_object.data || !img_scene.data )
  { std::cout<< " --(!) Error reading images " << std::endl; return -1; }
  
  //cv::equalizeHist(img_scene, img_scene);
  //my_convertToBinary(img_scene, img_scene); 
  //my_convertToBinary(img_object, img_object);

  //-- Step 1: Detect the keypoints using SURF Detector
  int minHessian = 10;

  //cv::SurfFeatureDetector detector( minHessian );
   //cv::SiftFeatureDetector detector( 1500);
  cv::SurfFeatureDetector detector(300, 5, 3);
  std::vector<KeyPoint> keypoints_object, keypoints_scene;

  detector.detect( img_object, keypoints_object );
  std::cout << keypoints_object.size() << "   ";
  detector.detect( img_scene, keypoints_scene );

  //-- Step 2: Calculate descriptors (feature vectors)
  //cv::SiftDescriptorExtractor extractor;
  SurfDescriptorExtractor extractor;

  Mat descriptors_object, descriptors_scene;

  extractor.compute( img_object, keypoints_object, descriptors_object );
  extractor.compute( img_scene, keypoints_scene, descriptors_scene );

  //-- Step 3: Matching descriptor vectors using FLANN matcher
  FlannBasedMatcher matcher;
  std::vector< DMatch > matches;
  matcher.match( descriptors_object, descriptors_scene, matches );

  double max_dist = 0; double min_dist = 100;

  //-- Quick calculation of max and min distances between keypoints
  for( int i = 0; i < descriptors_object.rows; i++ )
  { double dist = matches[i].distance;
    if( dist < min_dist ) min_dist = dist;
    if( dist > max_dist ) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist );
  printf("-- Min dist : %f \n", min_dist );

  //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
  std::vector< DMatch > good_matches;

  for( int i = 0; i < descriptors_object.rows; i++ )
  { if( matches[i].distance < 3*min_dist )
     { good_matches.push_back( matches[i]); }
  }
 // if(min_dist == 0) good_matches = matches;
 good_matches = matches;

  Mat img_matches;
  drawMatches( img_object, keypoints_object, img_scene, keypoints_scene,
               good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
               vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
  

  //-- Localize the object
  std::vector<Point2f> obj;
  std::vector<Point2f> scene;

  for( int i = 0; i < good_matches.size(); i++ )
  {
    //-- Get the keypoints from the good matches
    obj.push_back( keypoints_object[ good_matches[i].queryIdx ].pt );
    scene.push_back( keypoints_scene[ good_matches[i].trainIdx ].pt );
  }

  if(good_matches.size() >= 1)
  {
  Mat H = findHomography( obj, scene, CV_RANSAC, 10);

  //-- Get the corners from the image_1 ( the object to be "detected" )
  std::vector<Point2f> obj_corners(4);
  obj_corners[0] = cvPoint(0,0); obj_corners[1] = cvPoint( img_object.cols, 0 );
  obj_corners[2] = cvPoint( img_object.cols, img_object.rows ); obj_corners[3] = cvPoint( 0, img_object.rows );
  std::vector<Point2f> scene_corners(4);

  perspectiveTransform( obj_corners, scene_corners, H);

  //-- Draw lines between the corners (the mapped object in the scene - image_2 )
  line( img_matches, scene_corners[0] + Point2f( img_object.cols, 0), scene_corners[1] + Point2f( img_object.cols, 0), Scalar(0, 255, 0), 4 );
  line( img_matches, scene_corners[1] + Point2f( img_object.cols, 0), scene_corners[2] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 );
  line( img_matches, scene_corners[2] + Point2f( img_object.cols, 0), scene_corners[3] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 );
  line( img_matches, scene_corners[3] + Point2f( img_object.cols, 0), scene_corners[0] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 );
  }
  //-- Show detected matches
  imshow( "Good Matches & Object detection", img_matches );

  waitKey(0);
  return 0;
  }

  /** @function readme */
  void readme()
  { std::cout << " Usage: ./SURF_descriptor <img1> <img2>" << std::endl; }
